
%%
close all
clear all
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculation of 3D Scenes with knowledge of camera matrices %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Load Images
%left_image  = imread('shed1.png');
%right_image = imread('shed2.png');
%% Get Points 20 Points
%[x,xp] = select_corr(20,left_image,right_image);
load('shed_x.mat')%loads all!
%% Show Points and Correspondence
imshow([left_image zeros(size(left_image,1),5) right_image]); hold on;
title('Corresponding Points')
step = 1; 
plot(x(1:step:end,1), x(1:step:end,2), '*y');
plot(xp(1:step:end,1)+size(left_image,2)+5, xp(1:step:end,2), '*y');
line([x(1:step:end,1) xp(1:step:end,1)+size(left_image,2)+5]', [x(1:step:end,2) xp(1:step:end,2)]', 'Color', 'r');
%% Obtain F
[x_h,xp_h] = euclid2hmg(x,xp);%converts  (x,y)|->(x,y,1)
%%%%%%%%%%%%%Normalized 8pt%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[x_hn,T1] = hmg2norm(x_h);%nomralizes coordinates using normPt = T1*Pt
[xp_hn,T2] = hmg2norm(xp_h);%x_h and xp_h with average distance to the centroid of 1.4142
A = get_A(x_hn,xp_hn);%A will have n rows and 9 columns
Fn = FfromA(A);%normalized F
F = T2'*Fn*T1; %get the denormalized fundamental matrix and epipoles
%%%%%%%%%%%%%RANSAC%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%:|
%% Load Known Calibration Matrices --- FOR SHED IMAGES, RANSAC and SIFT NEEDED
Ks = load('K1_K2.mat');
Kp = Ks.K2;
K = Ks.K1;
E = Kp'*F*K;      %obtain essential matrix!
%% Estimate the Calibrated Camera Matrices
%P = [I|0], calculate Pp
Pcam = [eye(3) zeros(3,1)];
pt = 9;         %point to use to calculate the correct Pp matrix, debugging purposes
[Ppcam ~] = PpfromKs(E,Pcam,K,Kp,x_h(pt,:),xp_h(pt,:));
P = K*Pcam;
Pp = Kp*Ppcam;
%% Get 3D Points, plot them
X1 = findTriangulation2(P,Pp,x_h,xp_h);X = X1(1:3,:);
figure,
hold on; 
plot3(X(1,[1 2 3 4]),X(2,[1 2 3 4]),X(3,[1 2 3 4]),'r*');
plot3(X(1,[5 13 16]),X(2,[5 13 16]),X(3,[5 13 16]),'k*');
plot3(X(1,6),X(2,6),X(3,6),'g*');
plot3(X(1,[11]),X(2,[11]),X(3,[11]),'m*');
line([X(1,1) X(1,2)],[X(2,1) X(2,2)],[X(3,1) X(3,2)])
line([X(1,2) X(1,3)],[X(2,2) X(2,3)],[X(3,2) X(3,3)])
line([X(1,1) X(1,5)],[X(2,1) X(2,5)],[X(3,1) X(3,5)])
line([X(1,5) X(1,6)],[X(2,5) X(2,6)],[X(3,5) X(3,6)])
line([X(1,3) X(1,4)],[X(2,3) X(2,4)],[X(3,3) X(3,4)])
line([X(1,4) X(1,13)],[X(2,4) X(2,13)],[X(3,4) X(3,13)])
line([X(1,5) X(1,13)],[X(2,5) X(2,13)],[X(3,5) X(3,13)])
line([X(1,2) X(1,11)],[X(2,2) X(2,11)],[X(3,2) X(3,11)])
axis equal;grid on;view(200,-80)
figure, 
imshow(left_image); hold on;
plot(x(11,1), x(11,2), 'm*');
plot(x([6],1), x([6],2), 'g*');
plot(x([1 2 3 4],1), x([1 2 3 4],2), 'r*');
plot(x([5 16],1), x([5 16],2), 'k*');
%% Plot Epipolar Correspondence
figure,
subplot(1,2,1), imagesc(left_image); title('Left Image, click to see corresponing epipolar line');colormap(gray);axis image;
subplot(1,2,2), imagesc(right_image); title('Right Image, epipolar lines');colormap(gray);axis image;
for i=1:4%try it only 4 times...
    subplot(1,2,1)%left image
    [px, py] = ginput(1);
    hold on; plot(px,py,'ro');
    m = [px py 1];%to homogeneous
    l = F*m'; %points in right image
    subplot(1,2,2)
    plotline(l);
end
pause(2)
close all

